#include "fuction_temp.hpp"
// #include <pcl/features/normal_3d.h>
// #include <pcl/sample_consensus/ransac.h>
// #include <pcl/search/kdtree.h>
#include <cmath>
//#include <transforms3d.h>

inline double my_abs(double a)
{
    if(a>=0)return a;
    else return -a;
}

//void copy_point(pcl::PointCloud<pcl::PointXYZ> &cloud,pcl::PointCloud<pcl::PointXYZ> &dscloud)
////去掉车体中心轴点
void move_stable_point(pcl::PointCloud<pcl::PointXYZ> &cloud,pcl::PointCloud<pcl::PointXYZ> &dscloud)
{
  std::size_t size;
  size = cloud.points.size();
  for(int i=0;i<size;i++)
  {
    if(!(((cloud.points[i].x<0)&&(cloud.points[i].x>-1.5))&&((cloud.points[i].y>0)&&(cloud.points[i].y<1.5))))
           dscloud.points.push_back(cloud.points[i]);
  }
}
void move_st_point(pcl::PointCloud<pcl::PointXYZ>::Ptr cloud,pcl::PointCloud<pcl::PointXYZ>::Ptr dscloud)
{
  std::size_t size;
  size = cloud->points.size();
  for(int i=0;i<size;i++)
  {
    if(!(((cloud->points[i].x<0)&&(cloud->points[i].x>-1.5))&&((cloud->points[i].y>0)&&(cloud->points[i].y<1.5))))
           dscloud->points.push_back(cloud->points[i]);
  }
  // 设置目标点云的高度和宽度
  dscloud->width = static_cast<int>(dscloud->points.size());
  dscloud->height = 1;
}